#!/usr/bin/env python3
import numpy as np
from blue.alg import GpsFusion1D
from blue.env import GpsF1DEnv
from blue.display import GpsF1DDisplay

if __name__=="__main__":
    # Init data
    start_p = 100
    start_v = 0
    acc = [-3,0,0,0,0,0,0,0,0,0,-3,0,0,0,0,0,0,0,0,0,3,0,0,0,0,0,0,0,0,0,3,0,0,0,0,0,0,0,0,0,3,0,0,0,0,0,0,0,0,0,3,0,0,0,0,0,0,0,0,0,-3,0,0,0,0,0,0,0,0,0,-3,0,0,0,0,0,0,0,0,0,]
    # Build Env
    env = GpsF1DEnv(0.5,20)
    real_pos,meas_pos,real_vel,real_acc,meas_acc = env.deal(start_p,start_v,acc)

    esti_pos = [start_p]
    esti_vel = [start_v]
    # Build estimater
    estimater = GpsFusion1D()
    for i in range(1,len(meas_pos)):
        # Run a step
        estimater.a = meas_acc[i-1]
        estimater.delta_t = 1
        ep = estimater.step(meas_pos[i])
        # Do something
        esti_pos.append(ep[0])
        esti_vel.append(ep[1])

    # Show result
    display = GpsF1DDisplay()
    display.drawPos(real_pos,meas_pos,esti_pos)
    display.drawVel(real_vel,esti_vel)
    display.show()